// Copyright (c) 2012, 2013, 2014 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <cstdlib>

#include "openMVG/sfm/pipelines/global/sfm_global_engine_relative_motions.hpp"
#include "openMVG/system/timer.hpp"
#include "openMVG/cameras/Cameras_Common_command_line_helper.hpp"
#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/cameras/Camera_Pinhole_Radial.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"

using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::cameras;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

std::string sDataRootDir{ "D:/dataset/qinghuaxuetang-colmap/" };
std::string sOutDir{ sDataRootDir + "/model/" };
std::string sSparseCameras{ "/sparse/cameras.txt" };
std::string sSparseImages{ "/sparse/images.txt" };
std::string sSparsePoints{ "/sparse/points3D.txt" };

// merge intrinsic ids
std::string sfm_json_converted{ sDataRootDir + "/model/sfm_data_converted.json"};
std::string sfm_json_init{ sDataRootDir + "/matches/sfm_data.json" };
std::string sfm_json{ sDataRootDir + "/reconstruction/sfm_data.json" };


// convert from bundler
std::string sImgNameList;
std::string sBundlerOutFile;

using namespace std;

struct Image
{
	int image_id;
	int camera_id;
	Mat3 r;
	Vec3 t;
	std::string img_name;
};

struct Intrinsic
{
	double w, h, f, px, py;
	double d[3];
	Intrinsic(double _w, double _h, double _f, double _px, double _py, double _d0, double _d1, double _d2) :w(_w), h(_h), f(_f), px(_px), py(_py){
		d[0] = _d0; d[1] = _d1; d[2] = _d2;
	}
	Intrinsic(){ w = h = f = px = py = d[0] = d[1] = d[2] = 0.0; }
	~Intrinsic(){}
};


bool MergeIntrinsicId(){
	// merge instrinsics id
	std::map<std::string, int> intrinsic_id_sets;
	SfM_Data sfm_data_init, sfm_data_converted;
	if (!Load(sfm_data_init, sfm_json_init, ESfM_Data(VIEWS | INTRINSICS))) {
		std::cerr << std::endl
			<< "The input SfM_Data file \"" << sfm_json_init << "\" cannot be read." << std::endl;
		return EXIT_FAILURE;
	}
	for (const auto& view : sfm_data_init.views){
		const string filename = stlplus::filename_part(view.second->s_Img_path);
		intrinsic_id_sets[filename] = view.second->id_intrinsic;
	}
	std::cout << "find total views in sfm_data_init: " << sfm_data_init.views.size() << std::endl;

	if (!Load(sfm_data_converted, sfm_json_converted, ESfM_Data(ALL))) {
		std::cerr << std::endl
			<< "The input SfM_Data file \"" << sfm_json_converted << "\" cannot be read." << std::endl;
		return EXIT_FAILURE;
	}
	std::cout << "find total views in sfm_data_converted: " << sfm_data_converted.views.size() << std::endl;

	Intrinsics& intrinsics = sfm_data_converted.intrinsics;
	Intrinsics intr_back = intrinsics;
	intrinsics.clear();
	for (auto& view : sfm_data_converted.views){
		const string filename = stlplus::filename_part(view.second->s_Img_path);
		// have this image name
		if (intrinsic_id_sets.find(filename) != intrinsic_id_sets.end()) {
			int id = intrinsic_id_sets[filename];
			int old_id = view.second->id_intrinsic;
			// do not have this intrinsic, then make a new one
			if (intrinsics.find(id) == intrinsics.end()){
				intrinsics[id] = intr_back[old_id];
			}
			view.second->id_intrinsic = id;
		}
		else {
			std::cerr << " do not have this image name in intrinsic_id_sets: " << filename << std::endl;
		}
	}
	std::cout << "views size: " << sfm_data_converted.views.size() << std::endl;
	std::cout << "intrinsics size: " << sfm_data_converted.intrinsics.size() << std::endl;
	std::cout << "extrinsics size: " << sfm_data_converted.poses.size() << std::endl;
	sfm::Save(sfm_data_converted, stlplus::create_filespec(sOutDir, "sfm_data_merged", ".json"), ESfM_Data(ALL));
	return EXIT_SUCCESS;
}

bool ConstructMatchPair(){
	SfM_Data sfm_data;;
	if (!Load(sfm_data, sfm_json, ESfM_Data(VIEWS))) {
		std::cerr << std::endl
			<< "The input SfM_Data file \"" << sfm_json << "\" cannot be read." << std::endl;
		return EXIT_FAILURE;
	}
	std::vector<bool> sfm_view_id(2000, false);
	int cnt = 0;
	for (const auto& view : sfm_data.views){
		sfm_view_id[view.first] = true;
		cnt++;
	}
	std::cout << "load sfm_data views number: " << cnt << std::endl;

	std::ifstream ifs;
	std::ofstream ofs;
	// construct match pair(openMVG) from points3D.txt
	std:string pair_file{ sOutDir + "/matches.f.txt" };
	ifs.open(sDataRootDir + sSparsePoints);
	if (!ifs.is_open()){
		std::cerr << "open file fail: " + pair_file << std::endl;
		return EXIT_FAILURE;
	}
	std::vector<std::set<int>> pairs(2000);
	std::set<int> total_ids;
	//# 3D point list with one line of data per point:
	//#   POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)
	//# Number of points: 416352, mean track length: 5.35296
	std::string str_line;
	std::vector<int> image_ids;
	while (getline(ifs, str_line)){
		image_ids.clear();
		istringstream iss(str_line);
		int id, r, g, b;
		float x, y, z, err;
		iss >> id >> x >> y >> z;
		iss >> r >> g >> b >> err;
		while (iss >> id){
			total_ids.insert(id - 1);
			if (sfm_view_id[id - 1]){
				image_ids.push_back(id - 1);
			}
			else {
				std::cerr << "this id is not in the sfm_data views: " << id - 1 << std::endl;
			}
			iss >> id;
		}

		std::sort(image_ids.begin(), image_ids.end());
		for (int i = 1; i < image_ids.size(); ++i){
			if (pairs.size() < image_ids[0]){
				pairs.resize(2 * pairs.size());
			}
			pairs[image_ids[0]].insert(image_ids[i]);
		}
	}
	std::cout << "total ids in cameras.txt " << total_ids.size() << std::endl;
	auto lambda = [&](size_t r, const std::set<int>& s){ return r + s.size(); };
	size_t pairs_num = accumulate(pairs.cbegin(), pairs.cend(), size_t(0), lambda);
	std::cout << "there are total " << pairs_num << " pairs" << std::endl;

	ofs.open(pair_file);
	if (!ofs.is_open()){
		std::cerr << "open file fail: " + pair_file << std::endl;
		return EXIT_FAILURE;
	}
	for (int i = 0; i < pairs.size(); ++i) {
		for (const int j : pairs[i]) {
			if (j != i){
				ofs << i << " " << j << std::endl;
        ofs << 1 << std::endl;
        ofs << 0 << " " << 0 << std::endl;
			}
		}
	}
	ofs.close();
	return EXIT_SUCCESS;
}

bool ConvertToOpenmvg(){
	// Load input SfM_Data scene
	SfM_Data sfm_data;
	sfm_data.s_root_path = sDataRootDir + "images/";
	std::ifstream ifs;
	std::ofstream ofs;
	// read sparse cameras
	//# Camera list with one line of data per camera:
	//#   CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]
	//# Number of cameras: 1
	//1 SIMPLE_RADIAL 3072 2304 2559.68 1536 1152 - 0.0204997
	std::vector<Intrinsic> intrinsics(2000);
	std::string str_temp;
	const std::string sparse_cameras{ sDataRootDir + sSparseCameras };
	ifs.open(sparse_cameras);
	if (!ifs.is_open()) {
		std::cout << "open file fail: " << sparse_cameras << std::endl;
		return EXIT_FAILURE;
	}
	getline(ifs, str_temp);//header
	getline(ifs, str_temp);//header
	getline(ifs, str_temp);//header
	int number = 0;
	while (ifs >> number) {
		ifs >> str_temp; //SIMPLE_RADIAL
		static double w, h, f, px, py, d;
		ifs >> w >> h >> f >> px >> py >> d;
		sfm_data.intrinsics[number - 1] = std::make_shared<Pinhole_Intrinsic_Radial_K3>(w, h, f, px, py, d, 0.0, 0.0);
		intrinsics[number - 1] = Intrinsic{ w, h, f, px, py, d, 0.0, 0.0 };
	}
	ifs.close();
	std::cout << "read sparse cameras number: " << sfm_data.intrinsics.size() << std::endl;

	// read sparse images
	//# Image list with two lines of data per image:
	//#   IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME
	//#   POINTS2D[] as(X, Y, POINT3D_ID)
	//# Number of images : 128, mean observations per image : 2550.9
	std::map<int, Image> images;
	const string sparse_images{ sDataRootDir + sSparseImages };
	ifs.open(sparse_images);
	if (!ifs.is_open()) {
		std::cout << "open file fail: " << sparse_images << std::endl;
		return EXIT_FAILURE;
	}

	getline(ifs, str_temp);//header
	getline(ifs, str_temp);//header
	getline(ifs, str_temp);//header
	getline(ifs, str_temp);//header

	while (ifs >> number) {
		static double qw, qx, qy, qz, tx, ty, tz;
		static int id;
		ifs >> qw >> qx >> qy >> qz >> tx >> ty >> tz;
		ifs >> id;
		ifs >> str_temp;

		Image img;
		int k = number - 1;
		img.image_id = k;
		img.camera_id = id - 1;
		img.r = Quaternion{ qw, qx, qy, qz };
		img.t = Vec3{ tx, ty, tz };
		img.img_name = stlplus::filename_part(str_temp);

		// construct sfm_data
		geometry::Pose3 pose{ img.r, -img.r.transpose() * img.t };
		sfm_data.poses[k] = pose;

		std::shared_ptr<View> ptr_view = std::make_shared<View>();
		ptr_view->id_intrinsic = img.camera_id;
		ptr_view->id_pose = k;
		ptr_view->s_Img_path = img.img_name;
		const auto& intrinsic = intrinsics[ptr_view->id_intrinsic];
		ptr_view->ui_width = intrinsic.w;
		ptr_view->ui_height = intrinsic.h;
		ptr_view->id_view = k;
		sfm_data.views[k] = ptr_view;

		getline(ifs, str_temp);//header
		getline(ifs, str_temp);//header
	}
	ifs.close();
	std::cout << "read sparse images number: " << images.size() << std::endl;

	std::cout << "views size: " << sfm_data.views.size() << std::endl;
	std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
	std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;

	// save to json format
	sfm::Save(sfm_data, stlplus::create_filespec(sOutDir, "sfm_data_converted", ".json"), ESfM_Data(ALL));
}

/*
// use only bundler output data
bool ConvertFromBundler(){
	// Load input SfM_Data scene
	SfM_Data sfm_data;
	sfm_data.s_root_path = sDataRootDir;

	std::vector<std::string> img_names;
	img_names.reserve(8000);
	std::vector<int> img_height_width;
	img_height_width.reserve(16000);

	std::ifstream ifs(sDataRootDir + sImgNameList, std::ios::in);
	if (!ifs.is_open()) {
		std::cout << "open file fail: " << sDataRootDir + sImgNameList << std::endl;
		return EXIT_FAILURE;
	}

	std::string img_name_tmp;
	int h, w;
	while (ifs >> img_name_tmp) {
		ifs >> h >> w;
		img_height_width.push_back(h);
		img_height_width.push_back(w);
		img_names.push_back("images/" + img_name_tmp);
	}
	std::cout << "load image name: " << img_names.size() << std::endl;
	std::cout << "load image size: " << img_height_width.size() << std::endl;
	ifs.close();



	// read bundler data
	int num_camera, num_point;
	std::ifstream instream(sDataRootDir + sBundlerOutFile, std::ios::in);
	if (!instream.is_open()) {
		std::cout << "open bundler fail: " << sDataRootDir + sBundlerOutFile << std::endl;
		return EXIT_FAILURE;
	}

	std::string line_in_file;
	getline(instream, line_in_file);//header
	instream >> num_camera >> num_point;

	if (img_names.size() != num_camera) {
		std::cout << "num_camera error: " << num_camera << std::endl;
		return EXIT_FAILURE;
	}

	// read camera parameter
	for (int i = 0; i < num_camera; ++i) {
		int h = img_height_width[2 * i];
		int w = img_height_width[2 * i + 1];
		std::shared_ptr<View> ptr_view = std::make_shared<View>();
		ptr_view->id_intrinsic = image_intrinsic[i];
		ptr_view->id_pose = i;
		ptr_view->id_view = i;
		ptr_view->s_Img_path = img_names[i];
		ptr_view->ui_height = h;
		ptr_view->ui_width = w;
		sfm_data.views[i] = ptr_view;

		geometry::Pose3 pose;

		double f, k1, k2;
		instream >> f >> k1 >> k2;

		instream >> pose.rotation_(0, 0)
			>> pose.rotation_(0, 1)
			>> pose.rotation_(0, 2)
			>> pose.rotation_(1, 0)
			>> pose.rotation_(1, 1)
			>> pose.rotation_(1, 2)
			>> pose.rotation_(2, 0)
			>> pose.rotation_(2, 1)
			>> pose.rotation_(2, 2);

		Vec3 translation;
		instream >> translation[0] >> translation[1] >> translation[2];
		pose.center_ = -(pose.rotation().transpose() * translation);

		Mat3 D(Mat3::Identity());
		D(2, 2) = -1.0;
		pose.rotation_ = D*pose.rotation_;

		sfm_data.poses[i] = pose;

		//std::shared_ptr<cameras::IntrinsicBase> ptr_intrinsic = std::make_shared<Pinhole_Intrinsic_Radial_K3>
		//  ( w, h, f, w / 2.0, h / 2.0, 0.0, 0.0, 0.0 );  // setup no distortion as initial guess
		//sfm_data.intrinsics[i] = ptr_intrinsic;
	}
	instream.close();

	std::cout << "views size: " << sfm_data.views.size() << std::endl;
	std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
	std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;


	// save to json format
	Save(sfm_data, stlplus::create_filespec(sOutDir, "sfm_data_converted", ".json"), ESfM_Data(ALL));
	//return 0;


	return EXIT_SUCCESS;
}
*/


int main(int argc, char **argv)
{
#if 0
    SfM_Data sfm_data;
    if (!Load(sfm_data, "D:/dataset/qinghuaxuetang/reconstruction/sfm_data.bin", ESfM_Data(ALL))) {
      std::cerr << std::endl
        << "The input SfM_Data file cannot be read." << std::endl;
      return EXIT_FAILURE;
    }

    Save(sfm_data, stlplus::create_filespec("D:/dataset/qinghuaxuetang/reconstruction/", "sfm_data", ".json"), ESfM_Data(ALL));
    return EXIT_SUCCESS;
#endif
  



	char task_id = '0';
	std::cout << "input argc: " << argc << std::endl;
	for (int i = 0; i < argc; ++i) {
		std::cout << argv[i] << std::endl;
	}
	task_id = *argv[1];

	if (argc > 2){
		sDataRootDir = std::string{ argv[2] };
	}
	
	std::cout << "\nYou called\n" << string(argv[0]) << std::endl;
	std::cout << "task id: " << task_id << std::endl;
	std::cout << "data root dir: " << sDataRootDir << std::endl;
	
	switch (task_id)
	{
		case '0': 
			ConvertToOpenmvg(); 
			break;
		case '1': 
			MergeIntrinsicId(); 
			break;
		case '2': 
			ConstructMatchPair(); 
			break;
		default: break;
	}

	return EXIT_SUCCESS;
}

